#include "PID.h"
#include "main.h"

CascadePID mypid = {0};

void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut)
{
  pid->kp = p;      // 比例
  pid->ki = i;      // 积分
  pid->kd = d;      // 微分
  pid->maxIntegral = maxI;
  pid->maxOutput = maxOut;
}

// 进行一次pid计算
// 参数为(pid结构体，目标值，反馈值)，计算结果放在pid结构体的output成员中
void PID_Calc(PID *pid, float reference, float feedback)
{
  // 更新数据
  pid->lastError = pid->error;        // 将旧error存起来
  pid->error = reference - feedback;  // 计算新error
  // 计算微分
  float dout = (pid->error - pid->lastError) * pid->kd;
  // 计算比例
  float pout = pid->error * pid->kp;
  // 计算积分
  pid->integral += pid->error * pid->ki;
  // 积分限幅
  if (pid->integral > pid->maxIntegral)
  {
    pid->integral = pid->maxIntegral;
  } else if (pid->integral < -pid->maxIntegral)
  {
    pid->integral = -pid->maxIntegral;
  }
  // 计算输出
  pid->output = pout + dout + pid->integral;
  // 输出限幅
  if (pid->output > pid->maxOutput)
  {
    pid->output = pid->maxOutput;
  } else if (pid->output < -pid->maxOutput)
  {
    pid->output = -pid->maxOutput;
  }
}

/**
 * @brief 
 * 
 * @param pid 
 * @param outerRef 外环目标值（位置）
 * @param outerFdb 外环反馈值 (位置)
 * @param innerFdb 内环反馈值 (速度)
 */
void PID_CascadeCalc(CascadePID *pid, float outerRef, float outerFdb, float innerFdb)
{
  // 位置模式
  PID_Calc(&pid->outer, outerRef, outerFdb);    // 计算外环
  PID_Calc(&pid->inner, pid->outer.output, innerFdb); // 计算内环
  pid->output = pid->inner.output;
  // 速度模式
  // PID_Calc(&pid->inner, 5, innerFdb); // 计算内环
  
}
